﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace ProjectRoughWork
{
    class NavMesh
    {
        /// <summary>
        /// Default Constructor
        /// </summary>

        public NavMesh()
        {
            nodes = new List<Node>();
            connections = new List<WeightedEdge>();
            rand = new Random();
            lastNodeID = 1;
        }

        public NavMesh(NavMesh copy)
        {
            nodes = new List<Node>();
            connections = new List<WeightedEdge>();
            rand = new Random();
            lastNodeID = 1;

            for (int index = 0; index < copy.nodes.Count; index++)
            {
                nodes.Add(new Node(copy.nodes[index].Position));
            }

            WeightedEdge temp, newConnection;
            int nodeID1, nodeID2;

            for (int index = 0; index < copy.connections.Count; index++)
            {
                temp = copy.connections[index];
                nodeID1 = temp.getConnectedNodes()[0].ID - 1;
                nodeID2 = temp.getConnectedNodes()[1].ID - 1;

                newConnection = new WeightedEdge(nodes[nodeID1], nodes[nodeID2]); 
                connections.Add(newConnection);
                nodes[nodeID1].addConnection(newConnection);
                nodes[nodeID2].addConnection(newConnection);
            }


        }

        /// <summary>
        /// Adds a node to the graph/mesh
        /// </summary>
        /// <param name="newNode"></param>

        public void addNode(Node newNode)
        {
            newNode.ID = lastNodeID;
            lastNodeID++;
            nodes.Add(newNode);
        }

        /// <summary>
        /// Removes a node from the graph/mesh
        /// </summary>
        /// <param name="target"></param>

        public void RemoveNode(Node target)
        {
            nodes.Remove(target);
        }

        /// <summary>
        /// Adds an edge/connection to the graph/mesh
        /// </summary>
        /// <param name="newConnection"></param>

        public void addConnection(WeightedEdge newConnection)
        {
            if (!connections.Contains(newConnection))
            {
                connections.Add(newConnection);
            }
        }

        /// <summary>
        /// Retrieves a random node's position in the graph 
        /// </summary>
        /// <returns>A random node's position</returns>

        public Vector3 getRandomPosition()
        {
            return nodes[rand.Next(nodes.Count - 1)].Position;
        }

        /// <summary>
        /// Finds a path to the target node from the given start node, using an A* Algorithm and Euclidian Heuristic calculation
        /// </summary>
        /// <param name="position"></param>
        /// <param name="targetPosition"></param>
        /// <param name="proximity"></param>
        /// <returns></returns>

        public Queue<Node> PathFind(Vector3 position, Vector3 targetPosition, float proximity)
        {
            //Scrub the path finding info of the nodes clean

            foreach (Node node in nodes)
            {
                node.CostSoFar = 0;
                node.EstimatedTotalCost = 0;
                node.Heuristic = 0;
                node.OptimalPath = null;
            }

            //Declaration and initialization of variables needed for the algorithm

            List<Node> path = new List<Node>();
            List<Node> openList = new List<Node>();
            List<Node> closedList = new List<Node>();

            Node oldNode, currentNode, tempNode;
            oldNode = null;
            float smallestEstimate = 9999f;
            currentNode = FindClosestNode(position);
            targetPosition = FindClosestNode(targetPosition).Position;

            while ( (currentNode.Position != targetPosition) &&
                    ((currentNode.Position - targetPosition).Length() > proximity)
                    )
            {
                if (oldNode == currentNode)
                {
                    int index = 0;
                }

                List<WeightedEdge> connections = currentNode.Connections;

                foreach (WeightedEdge connection in connections)
                {
                    //Retrieve nodes connected to the current node

                    tempNode = connection.adjacentNode(currentNode);
                    connection.ConnectionColor = Color.Green;

                    //Check if the node is already on the closed list

                    if (closedList.Contains(tempNode))
                    {
                        //If the new estimated cost so far is better than the old one, put node back on the open list

                        if (currentNode.CostSoFar + connection.Weight < tempNode.CostSoFar)
                        {
                            closedList.Remove(tempNode);
                            tempNode.CostSoFar = currentNode.CostSoFar + connection.Weight;
                            tempNode.EstimatedTotalCost = tempNode.CostSoFar + tempNode.Heuristic;
                            tempNode.OptimalPath = currentNode;
                            openList.Add(tempNode);

                        }
                    }
                    else if (openList.Contains(tempNode))
                    {
                        //same as above but for the open list

                        if (currentNode.CostSoFar + connection.Weight < tempNode.CostSoFar)
                        {
                            tempNode.CostSoFar = currentNode.CostSoFar + connection.Weight;
                            tempNode.EstimatedTotalCost = tempNode.CostSoFar + tempNode.Heuristic;
                            tempNode.OptimalPath = currentNode;
                        }
                    }
                    else 
                    {
                        //Calculates cost so far, heuristic and estimated total cost for the node as well as sets its optimal path node

                        tempNode.CostSoFar = currentNode.CostSoFar + connection.Weight;
                        tempNode.Heuristic = getHeuristic(targetPosition, tempNode.Position);
                        tempNode.EstimatedTotalCost = tempNode.CostSoFar + tempNode.Heuristic;
                        tempNode.OptimalPath = currentNode;

                        openList.Add(tempNode);
                    }
                }

                closedList.Add(currentNode);
                oldNode = currentNode;

                //Loops through open list and sets the current node to the smallest estimated total cost of them all

                for (int index = openList.Count - 1; index >= 0; index--)
                {
                    if (openList[index].EstimatedTotalCost < smallestEstimate)
                    {
                        smallestEstimate = openList[index].EstimatedTotalCost;
                        currentNode = openList[index];
                    }
                }

                smallestEstimate = 9999;

                //Removes the new current node from the open list and adds it to the closed list

                openList.Remove(currentNode);
                //closedList.Add(currentNode);

                //oldNode.OptimalPath = currentNode;
            }

            closedList.Add(currentNode);

            return ExtractPath(closedList, FindClosestNode(position), currentNode);
        }

        /// <summary>
        /// Finds node closest to the given position
        /// </summary>
        /// <param name="position">Position to find a node closest to</param>
        /// <returns>Node closest to the given position</returns>

        public Node FindClosestNode(Vector3 position)
        {
            Node closest = null;
            float smallestDistance = 9999f;

            foreach (Node node in nodes)
            {
                if ((node.Position - position).Length() < smallestDistance)
                {
                    smallestDistance = (node.Position - position).Length();
                    closest = node;
                }
            }

            return closest;
        }

        public Vector3 FindSafeNode(Vector3 actorPosition, Vector3 target, int safeRange)
        {
            Node safeNode = null;
            float smallestDistance = 9999;

            while (safeNode == null)
            {
                foreach (Node node in nodes)
                {
                    if (((node.Position - target).Length() > safeRange) &&
                        ((node.Position - actorPosition).Length() < smallestDistance)
                        )
                    {
                        safeNode = node;
                        smallestDistance = (node.Position - actorPosition).Length();
                    }
                }
                safeRange += 20;
            }

            return safeNode.Position;
        }
        /// <summary>
        /// Calculates Heuristic using the Euclidian algorithm
        /// </summary>
        /// <param name="target">Position of the target/goal node</param>
        /// <param name="current">Position of the node being considered</param>
        /// <returns>Calculated heuristic value</returns>

        public float getHeuristic(Vector3 target, Vector3 current)
        {
            return Math.Abs((target - current).Length());
        }

        /// <summary>
        /// Extracts the path to the goal node, in the form of Queue of nodes
        /// </summary>
        /// <param name="closedList">The final closed list from the A* algorithm</param>
        /// <param name="startNode">The node path finded from the beginning of the algorithm</param>
        /// <param name="endNode">The goal node</param>
        /// <returns>A queue of the nodes to follow to the goal node</returns>

        public Queue<Node> ExtractPath(List<Node> closedList, Node startNode, Node endNode)
        {
            Stack<Node> reversedPath = new Stack<Node>();
            Queue<Node> path = new Queue<Node>();
            Node tempNode;

            tempNode = closedList.First(test => test == endNode);
           //tempNode = closedList.Find(delegate(Node test) { return (test == endNode); });

            //Backtrack from the goal node and push all the nodes on the path towards it onto the stack

           while (tempNode != startNode)
           {
               //path.Enqueue(tempNode);

               reversedPath.Push(tempNode);
               tempNode = tempNode.OptimalPath;
           }

           reversedPath.Push(startNode);

           //Use the created stack to create a proper queue to nodes to follow from the start node to get to the goal node

           while (reversedPath.Count > 0)
           {
               path.Enqueue(reversedPath.Pop());
           }

            return path;
        }

        public void TileGraphCleanUp()
        {
            for (int index = nodes.Count - 1; index >= 0; index--)
            {
                if (nodes[index].Connections.Count == 0)
                {
                    nodes.Remove(nodes[index]);
                }
            }
        }

        //Draws all the edges of the Navigation Mesh

        public void Draw()
        {
            float width = Game1.GetGraphicsDevice().Viewport.Width;
            float height = Game1.GetGraphicsDevice().Viewport.Height;
            float tempX, tempZ;

            SpriteBatch spriteBatch = Game1.GetSpriteBatch();
            SpriteFont spriteFont = Game1.GetHeaderFont();

            Effect effect = Game1.GetCustomEffect();
            effect.Parameters["World"].SetValue(Matrix.Identity);

            VertexPositionColor[] vertices = new VertexPositionColor[2];
            Node[] tempNodes;
            //Game1.GetGraphicsDevice().RenderState.PointSize = 20;

            foreach (WeightedEdge connection in connections)
            {
                tempNodes = connection.getConnectedNodes();

                vertices[0].Position = tempNodes[0].Position;
                vertices[0].Color = connection.ConnectionColor;

                vertices[1].Position = tempNodes[1].Position;
                vertices[1].Color = connection.ConnectionColor;

                foreach (EffectPass pass in effect.CurrentTechnique.Passes)
                {
                    pass.Apply();

                    Game1.GetGraphicsDevice().DrawUserPrimitives<VertexPositionColor>(PrimitiveType.LineList, vertices, 0, 1);
                }
                /*

                tempX = (vertices[0].Position.X + 400) / 800;
                tempZ = (vertices[0].Position.Z + 400) / 800;

                spriteBatch.Begin();

                spriteBatch.DrawString(spriteFont, tempNodes[0].ID.ToString(), new Vector2(tempX*width, tempZ*height), Color.Blue);

                tempX = (vertices[1].Position.X + 400) / 800;
                tempZ = (vertices[1].Position.Z + 400) / 800;

                spriteBatch.DrawString(spriteFont, tempNodes[1].ID.ToString(), new Vector2(tempX * width, tempZ * height), Color.Blue);
                
                spriteBatch.End();
                 
                */
            }
        }

        //Data Members

        List<Node> nodes;
        List<WeightedEdge> connections;

        int lastNodeID;
        Random rand;
    }
}
